from bzrc import BZRC, Command, ShootCommand
import numpy, sys, math, time, optparse, random, pprint, threading
from numpy import array, mat
from Queue import Queue, PriorityQueue as PQ
from PriorityQueue import PriorityQueue
from Gnuplot import GnuplotProcess
import plotsearch
import plot_grid

TIME_STEP = .36
FRIC_COEFF = .005
STDEV = 25
NUMBOTS = 12
F = mat([[1, TIME_STEP, TIME_STEP * TIME_STEP / 2, 0, 0, 0],
	[0, 1, TIME_STEP, 0, 0, 0],
	[0, -FRIC_COEFF, 1, 0, 0, 0],
	[0, 0, 0, 1, TIME_STEP, TIME_STEP * TIME_STEP / 2],
	[0, 0, 0, 0, 1, TIME_STEP],
	[0, 0, 0, 0, -FRIC_COEFF, 1]])
FT = F.T
H = mat([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
HT = H.T
EX = mat([[.1, 0, 0, 0, 0, 0], [0, .1, 0, 0, 0, 0], [0, 0, 10, 0, 0, 0], [0, 0, 0, .1, 0, 0], [0, 0, 0, 0, .1, 0], [0, 0, 0, 0, 0, 10]])
MUs = [mat(numpy.zeros((6, 1), float)) for _ in range(NUMBOTS+1)]
SIGMAs = [mat([[10, 0, 0, 0, 0, 0], [0, .1, 0, 0, 0, 0], [0, 0, .1, 0, 0, 0], [0, 0, 0, 10, 0, 0], [0, 0, 0, 0, .1, 0], [0, 0, 0, 0, 0, .1]]) for _ in range(NUMBOTS+1)]
I = mat(numpy.identity(6, float))
EZ = mat([[STDEV, 0], [0, STDEV]])
SHOT_SPEED = 100 #shot speed is about 100 meters a second
RANGE = 345
SEARCH_SIZE = 1600
NORMAL_COST = 1
MAX_COST = 15
PEN_DIST = MAX_COST - 1
PENALIZE = True
INITIAL_SKIP = 15
KP = 1
KD = 2
NODE_DIST = 30
NODE_SKIP = 5
JIGGLE = .07

CALLSIGNS = {} # Holds a map of callsigns => indices
#Indices will point to the correct place in MU/SIGMA arrays

kalmanLock = threading.Lock() # A Lock for the MU and SIGMA arrays
kalmanUpdate = Queue() # Should be bot objects -- handles blocking automatically like a Semaphore

class Agent(object):
	def __init__(self, bzrc):
		self.bzrc = bzrc
		self.constants = self.bzrc.get_constants()
		print self.constants
		self.commands = []
		self.state = ["basic" for _ in range(NUMBOTS)]
		self.types = ["Seeker", "Decoy", "Seeker", "Decoy", "Seeker", "Decoy", "Seeker", "Decoy", "Seeker", "Decoy"]
		self.searcher = AStar(bzrc)
		self.myBase = [base for base in bzrc.get_bases() if base.color == self.constants["team"]][0]
		othertanks = self.bzrc.get_othertanks()
		self.enemyColor = othertanks[0].color
		self.gotFlag = False
		flagX = 0
		flagY = 0
		for _ in range(50): # Figure out where their flag really is
			flag = [flag for flag in bzrc.get_flags() if flag.color == self.enemyColor][0]
			flagX += flag.x
			flagY += flag.y
		flag.x = flagX / 50
		flag.y = flagY / 50
		self.otherFlag = flag
			
		self.guardPoints = []
		self.baseCenter = find_center(self.myBase.coords)
		self.worldSize = int(self.constants["worldsize"])
		print "Base center: ", self.baseCenter
		halfSize = int(self.constants['worldsize']) / 2
		for i in range(NUMBOTS - 2):
			angle = i * 2 * math.pi / (NUMBOTS - 2) - math.pi # Find a unique angle away from base
			position = [50*math.cos(angle) + self.baseCenter[0], 50*math.sin(angle) + self.baseCenter[1]]
			#Bounds checking time
			if position[0] < -halfSize + 10:
				position[0] = -halfSize + 10
			elif position[0] > halfSize + 10:
				position[0] = halfSize + 10
			if position[1] < -halfSize + 10:
				position[1] = -halfSize + 10
			elif position[1] > halfSize + 10:
				position[1] = halfSize + 10
			self.guardPoints.append(position) # March out about 50m
		self.path = {}
		self.curNode = range(NUMBOTS)
		self.targetNode = [0 for i in range(NUMBOTS)]
		self.lastTarget = range(NUMBOTS)
		self.lastAng = range(NUMBOTS)
		#self.gp = GnuplotProcess(persist=1)
		#self.gp.write(plotsearch.gnuplot_header(-halfSize, halfSize))
		#self.gp.write(plotsearch.start_multiplot())
		# Assign an index to each tank's callsign
		othertanks = self.bzrc.get_othertanks()
		index = 0

		global CALLSIGNS
		for tank in othertanks:
			CALLSIGNS[tank.callsign] = index
			index += 1

	def tick(self):
		'''Some time has passed; decide what to do next'''
		# Get information from the BZRC server
		print "Tick!"
		mytanks, othertanks, flags, shots = self.bzrc.get_lots_o_stuff()
		self.mytanks = mytanks
		self.shots = shots
		self.othertanks = othertanks
		self.ourFlag = [flag for flag in flags if flag.color == self.constants['team']][0]
		#print "Pushing tanks onto queue."
		for tank in othertanks:
			kalmanUpdate.put(tank)
		#print "Done pushing tanks onto queue."
		self.check_shots() # This will put any guards into Dodge mode when being shot at
		self.targets = PQ()
		self.check_enemies() # Figure out which enemies are primed to die

		self.flags = flags
		self.shots = shots
		# Reset my set of commands (we don't want to run old commands)
		self.commands = []

		# Decide what to do with my tank
		for tank in mytanks:
			if self.types[tank.index] == "Seeker":
				if (tank.status == "dead" and tank.index < len(mytanks) - 2):
					self.types[tank.index + 1] = "Seeker"
				self.seek(tank)
			elif self.types[tank.index] == "Decoy":
				self.distract(tank)
			else: #Guard
				self.guard(tank)
			command = ShootCommand(tank.index)
			self.commands.append(command)

		# Send the commands to the server
		self.bzrc.do_commands(self.commands)
		#plot_grid.draw_grid()
		return False

	def seek(self, bot):
		flagPos = (self.otherFlag.x, self.otherFlag.y)
		if self.state[bot.index] == "basic": # Seek
			if not self.path:
				command = Command(bot.index, .1, 1)
				self.commands.append(command)
				self.bzrc.do_commands(self.commands)
				self.commands = []
				self.path, self.targetNode[bot.index] = self.do_search(bot, flagPos)
				#self.gp.write(plotsearch.plot_nodes(self.path, bot.index))
				#self.gp.flush()
			self.follow_path(bot)
			dist = math.hypot(bot.x - flagPos[0], bot.y - flagPos[1])
			if dist < SEARCH_SIZE / 2:
				self.state[bot.index] = "Get Flag"
		elif self.state[bot.index] == "Get Flag":
			self.get_the_flag(bot)
			if self.otherFlag.poss_color == self.constants["team"]:
				self.state[bot.index] = "Return"
				print "Return"
		else:
			if not self.gotFlag:
				self.gotFlag = True
				for i in range(0, 10)
					self.targetNode[i] = 0
				self.path = self.path.reverse()
			self.follow_path(bot)
		for tank in self.othertanks: # Shoot to kill... if it's worth it
			with kalmanLock:
				enemyPos = MUs[CALLSIGNS[tank.callsign]]
			delY = enemyPos[3,0] - bot.y
			delX = enemyPos[1,0] - bot.x
			dist = math.hypot(delX, delY)
			if dist > RANGE: # Out of Range; don't fire
				continue
			delAng = math.atan2(delY, delX) - bot.angle
			if math.fabs(delAng) > .3: # We're not pointed at this one; we don't care too much
				continue
			command = ShootCommand(bot.index)
			self.commands.append(command)
			break # We're already shooting; no reason to check everyone else
			

	def distract(self, bot):
		flagPos = (self.otherFlag.x, self.otherFlag.y)
		if self.state[bot.index] == "basic": # Seek
			if not self.path:
				command = Command(bot.index, .1, 1)
				self.commands.append(command)
				self.bzrc.do_commands(self.commands)
				self.commands = []
				self.path, self.targetNode[bot.index] = self.do_search(bot, flagPos)
				#self.gp.write(plotsearch.plot_nodes(self.path[bot.index], bot.index))
				#self.gp.flush()
			self.follow_path(bot)
			dist = math.hypot(bot.x - flagPos[0], bot.y - flagPos[1])
			if dist < SEARCH_SIZE / 4:
				self.state[bot.index] = "Harass"
		elif self.state[bot.index] == "Harass":
			if not self.path[bot.index]:
				self.path[bot.index], self.targetNode[bot.index] = self.do_search(bot, flagPos)
			self.get_the_flag(bot)
		for tank in self.othertanks: # Shoot to kill... if it's worth it
			with kalmanLock:
				enemyPos = MUs[CALLSIGNS[tank.callsign]]
			delY = enemyPos[3,0] - bot.y
			delX = enemyPos[1,0] - bot.x
			dist = math.hypot(delX, delY)
			if dist > RANGE: # Out of Range; don't fire
				continue
			delAng = math.atan2(delY, delX) - bot.angle
			if math.fabs(delAng) > .08: # We're not pointed at this one; we don't care too much
				continue
			command = ShootCommand(bot.index)
			self.commands.append(command)
			break # We're already shooting; no reason to check everyone else

	def guard(self, bot):
		state = self.state[bot.index]
		if state == "dodge": # We're dodging a bullet; top priority
			command = Command(bot.index, -1, 1)
			self.commands.append(command)
		elif state == "basic": # Drive to initial point
			guardPoint = self.guardPoints[bot.index - 2]
			dist = math.hypot(bot.x - guardPoint[0], bot.y - guardPoint[1])
			print "Guard distance: ", dist
			if dist < 20: # We're here; change state and execute new orders
				self.state[bot.index] = "guard"
				self.guard(bot)
				return
			vel, ang = self.pd(bot, guardPoint)
			command = Command(bot.index, vel / 8, ang)
			self.commands.append(command)
		elif state == "guard":
			try:
				dist, target = self.targets.get_nowait() # Grab the target closest to the base that has not been selected
				self.aim(bot, target, dist) # Shoot to kill, men
			except: # Nobody worth shooting at
				command = Command(bot.index, random.uniform(-.3, .3), 0) # Jitter a little to screw with Kalman filters; we probably won't stray too far from home, but even if we do, we don't care
				self.commands.append(command)
	
	def check_shots(self):
		shotAt = {}
		for shot in self.shots:
			shotAng = math.hypot(shot.vx, shot.vy) + math.pi # Rotate the shot's angle 180 for easier checking vs the tanks
			for tank in self.mytanks:
				if self.types[tank.index] != "Guard":
					continue # These guys don't need to dodge
				tankAng = math.hypot(shot.x - tank.x, shot.y - tank.y) # Angle between the shot and the tank
				delAng = self.normalize_angle(tankAng - shotAng)
				if math.fabs(delAng) < .3:
					self.state[tank.index] = "dodge"
					shotAt[tank.index] = True
		for tank in self.mytanks: # Revert the guys who were dodging, but need not anymore
			if self.types[tank.index] != "Guard":
				continue
			if self.state[tank.index] == "dodge" and not tank.index in shotAt:
				self.state[tank.index] = "basic" # Move back to position

	def check_enemies(self):
		baseX = self.baseCenter[0]
		baseY = self.baseCenter[1]
		for enemy in self.othertanks:
			dist = math.hypot(enemy.x - baseX, enemy.y - baseY)
			if dist > RANGE+100:
				continue # We can't shoot at this thing, anyway. We don't care.
			self.targets.put((dist, enemy))

	def follow_path(self, bot):
		#print "Path: ", self.path
		nodeCoords = self.path[self.targetNode[bot.index]]
		#print "Target node: ", nodeCoords
		vel, ang = self.pd(bot, nodeCoords)
		
		command = Command(bot.index, vel, ang)
		self.commands.append(command)
		dist = math.hypot(bot.x - nodeCoords[0], bot.y - nodeCoords[1])
		if dist < NODE_DIST:
			self.targetNode[bot.index] += NODE_SKIP
			if self.targetNode[bot.index] >= len(self.path) - 1:
				self.targetNode[bot.index] = len(self.path) - 1
						

	def get_the_flag(self, bot):
		if self.targetNode[bot.index] >= len(self.path) - 1:
				self.path[bot.index] = len(self.path) - 1
		nodeCoords = self.path[self.targetNode[bot.index]]
		vel, ang = self.pd(bot, nodeCoords)
		command = Command(bot.index, vel/2, ang)
		self.commands.append(command)
		dist = math.hypot(bot.x - nodeCoords[0], bot.y - nodeCoords[1])
		if dist < NODE_DIST:
			self.targetNode[bot.index] += NODE_SKIP
			if self.targetNode[bot.index] >= len(self.path) - 1:
				self.path[bot.index] = len(self.path) - 1

	def aim(self, bot, target, distToTarget):
		MU = ()
		with kalmanLock:
			MU = MUs[CALLSIGNS[target.callsign]]

		timeToTarget = distToTarget / SHOT_SPEED + .5

		FPrime = mat([[1, timeToTarget, timeToTarget * timeToTarget / 2, 0, 0, 0],
			[0, 1, timeToTarget, 0, 0, 0],
			[0, -FRIC_COEFF, 1, 0, 0, 0],
			[0, 0, 0, 1, timeToTarget, timeToTarget * timeToTarget / 2],
			[0, 0, 0, 0, 1, timeToTarget],
			[0, 0, 0, 0, -FRIC_COEFF, 1]]) #Matrix for extrapolating position to when the shot will arrive

		projectedLocationMU = FPrime * MU
		projectedLocation = (projectedLocationMU[0,0], projectedLocationMU[3,0])

		desiredAngle = math.atan2(projectedLocation[1] - bot.y, projectedLocation[0] - bot.x)
		delAng = self.normalize_angle(desiredAngle - bot.angle)
		command = Command(bot.index, 0, delAng)
		self.commands.append(command)

		distToTarget = math.hypot(bot.x - projectedLocation[0], bot.y - projectedLocation[1])

		if(distToTarget > RANGE or math.fabs(delAng) > .04):
			return #Don't bother shooting at someone out of range or not in our line of sight
		command = ShootCommand(bot.index)
		self.commands.append(command)

	def do_search(self, bot, goal):
		path = self.searcher.search(translate_node_reverse((bot.x,bot.y), -self.worldSize / 2, -self.worldSize / 2), goal)
		if (len(path) > INITIAL_SKIP):
			return path, INITIAL_SKIP
		return path, 0

	def pd(self, bot, targetPos):
		targetAng = math.atan2(targetPos[1] - bot.y, targetPos[0] - bot.x)
		botAng = self.normalize_angle(bot.angle)
		delA = (targetAng - botAng) - (self.lastTarget[bot.index] - self.lastAng[bot.index])
		a = KP * (targetAng - botAng) + KD * delA
		self.lastTarget[bot.index] = targetAng
		self.lastAng[bot.index] = botAng
		a = self.normalize_angle(a)
		return 1 - math.fabs(a/math.pi) + random.uniform(-JIGGLE, JIGGLE), a + random.uniform(-JIGGLE, JIGGLE)

	def normalize_angle(self, angle):
		'''Make any angle be between +/- pi.'''
		angle -= 2 * math.pi * int (angle / (2 * math.pi))
		if angle <= -math.pi:
			angle += 2 * math.pi
		elif angle > math.pi:
			angle -= 2 * math.pi
		return angle


class Kalman(threading.Thread):

	def __init__(self):
		threading.Thread.__init__(self)
		threading.Thread.daemon = True

	def run(self):
		global MUs
		global SIGMAs
		while True:
			target = kalmanUpdate.get(True) # Wait until we have ought to update
			start = time.time()
			index = CALLSIGNS[target.callsign]
			MU = MUs[index]
			SIGMA = SIGMAs[index]
			v = mat([[(target.x - MU[0,0]) / TIME_STEP], [(target.y - MU[3,0]) / TIME_STEP]])
			a = mat([[(v[0,0] - MU[1,0]) / TIME_STEP, (v[1,0] - MU[4,0]) / TIME_STEP]]).T
			X = mat([(target.x, v[0,0], a[0,0], target.y, v[1,0], a[1,0])]).T
			Z = H * X
			FET = F * SIGMA * FT + EX
			K = FET * HT * (H * FET * HT + EZ).I
			FMU = F * MU
			MU = FMU + K * (Z - H * FMU)
			SIGMA = (I - K * H) * FET
			with kalmanLock:
				MUs[index] = MU
				SIGMAs[index] = SIGMA
			delTime = time.time() - start
			#print "Kalman filter took: ", delTime, " to compute."

class AStarNew:
	def __init__(self, bzrc):
		nodes, penalties = parse_grid(int(bzrc.get_constants()["worldsize"]), bzrc)
		self.backupNodes = nodes
		self.nodes = nodes
		self.startPos = (0,0)
		self.flagPos = (0,0)
		self.lastNode = (0,0)
		self.frontier = PriorityQueue()
		self.path = []
		self.searched = {}
		self.newNodes = []
		self.penalties = penalties
		self.bestSeenCost = 10000000000000
		self.bestSeen = (0,0)
		
	def search(self, curX, curY, goal):
		print "Running search..."
		self.flagPos = goal
		self.nodes = self.backupNodes
		self.lastNode = (curX, curY)
		self.startPos = (curX, curY)
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (curX, curY), None, 0) 
		self.path = []
		self.searched = {}
		self.newNodes = []
		exists = self.iter_search()
		if not exists:
			print "Error! Could not find path from ", self.startPos, " to ", self.flagPos
		allSearched = []
		for node in self.nodes:
			if node in self.searched:
				allSearched.append(node)
				
		finalPath = [exists]
		curNode = exists
		nextNode = self.searched[exists]
		while nextNode != None:
			finalPath.append(nextNode)
			curNode = nextNode
			nextNode = self.searched[curNode]
		print "Total nodes popped: ", len(allSearched)
		print "Length of final path: ", len(finalPath)
		return finalPath
		
	def step(self, steps):
		toReturn = []
		for i in range(steps):
			if not self.frontier:
				break
			done = self.iter_step()
			toReturn += self.newNodes
			if done:
				break
		return toReturn
		
	def iter_search(self):
		while self.frontier.has_item():
			answer = self.iter_step()
			if answer:
				return answer
		return False
		
	def iter_step(self):
		priority, cost, curNode, parent = self.frontier.dequeue()
		print "Node: ", curNode
		if not curNode:
			#print "Error!"
			return False
		if not curNode in self.searched:
			self.searched[curNode] = parent
		if (curNode[0], curNode[1]) == self.flagPos:
			return curNode
		self.newNodes = []
		self.check_node((curNode[0]+1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0], curNode[1]-1), cost, curNode)
		self.check_node((curNode[0], curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]), cost, curNode)
		return False
		
	def check_node(self, node, cost, parent):
		if node in self.nodes and self.nodes[node] and not node in self.searched:
			newCost = cost + self.calc_cost(node, parent)
			if not self.frontier.get_data_priority(node):
				self.frontier.enqueue(newCost + self.heuristic_cost(node), node, parent, newCost)
				self.newNodes.append(node)
			elif self.frontier.get_data_priority(node) > newCost + self.heuristic_cost(node):
				self.frontier.reprioritize(newCost + self.heuristic_cost(node), node, parent, newCost)
				if node in self.searched:
					self.searched[curNode] = parent
				
	def heuristic_cost(self, node):
		#Our heuristic is just straight-line distance
		dist = math.hypot(node[0] - self.flagPos[0], node[1] - self.flagPos[1])
		return dist
			
	def calc_cost(self, node, parent):
		if PENALIZE:
			return self.penalties[node[0],node[1]] * math.hypot(node[0] - parent[0], node[1] - parent[1])
		else:
			return NORMAL_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])
						
	
	def is_adjacent(self, node):
		if (node[0],node[1]+1) in self.nodes and (self.nodes[(node[0],node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]+1) in self.nodes and (self.nodes[(node[0]-1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]) in self.nodes and (self.nodes[(node[0]-1,node[1])] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]-1) in self.nodes and (self.nodes[(node[0]+1,node[1]-1)] == 0):
			return True
		if (node[0],node[1]-1) in self.nodes and (self.nodes[(node[0],node[1]-1)] == 0):
			return True
		if (node[0]-1,node[1]-1) in self.nodes and (self.nodes[(node[0]-1,node[1]-1)] == 0):
			return True
		return False
		
	def clear(self):
		self.path = []
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (self.startPos[0], self.startPos[1]), None, 0)
		self.searched = {}
		
def parse_gridNew(size, bzrc):
	toReturn = {}
	penaltyGrid = {}
	
	#Get the obstacles
	obstacles = bzrc.get_obstacles()
	otherTanks = bzrc.get_othertanks()
	
	for i in range(0, size):
		for j in range(0, size):
			toReturn[(int(i-size/2),int(j-size/2))] = 1
			penaltyGrid[(int(i-size/2),int(j-size/2))] = NORMAL_COST
	
	#Iterate through the obstacles, drawing line in between them
	for obstacle in obstacles:
		for i, point in enumerate(obstacle):
			#figure out which direction to go
			if i+1 < len(obstacle):
				point2 = obstacle[i+1]
			else:
				point2 = obstacle[0]
				
			diffx = point2[0] - point[0]
			diffy = point2[1] - point[1]
			
			rge = {}
			
			if diffx < 0:
				rge = range(0, int(diffx), -1)
			elif diffx > 0:
				rge = range(0, int(diffx), 1)
				
			if rge:
				for x in rge:
					toReturn[int(point[0])+x, int(point[1])] = 0
					for y in range(-PEN_DIST, PEN_DIST, 1):
						penaltyGrid[int(point[0])+x, int(point[1])+y] = MAX_COST - math.fabs(y)						
			else:
				if diffy < 0:
					rge = range(0, int(diffy), -1)
				else:
					rge = range(0, int(diffy), 1)
				for y in rge:
					toReturn[int(point[0]), int(point[1])+y] = 0
					for x in range(-PEN_DIST, PEN_DIST, 1):
						penaltyGrid[int(point[0])+x, int(point[1])+y] = MAX_COST - math.fabs(x)	
									
	return toReturn, penaltyGrid

class AStar:
	def __init__(self, bzrc):
		self.worldSize = int(bzrc.get_constants()["worldsize"])
		nodes, penalties = parse_grid(self.worldSize, bzrc)
		self.backupNodes = nodes
		self.backupPenalties = penalties
		self.nodes = {}
		self.startPos = (0, 0)
		self.flagPos = (0,0)
		self.lastNode = (0, 0)
		self.frontier = PriorityQueue()
		self.path = []
		self.searched = {}
		self.newNodes = []
		self.penalties = penalties
		self.bestSeenCost = 10000000000000
		self.bestSeen = (0,0)

	def search(self, botpos, goal):
		print "Running search...", botpos
		curX = botpos[0]
		curY = botpos[1]
		startTime = time.time()
		self.path = []
		self.searched = {}
		self.newNodes = []
		self.frontier = PriorityQueue()
		self.bestSeenCost = 10000000000000
		startSlicex = curX - SEARCH_SIZE / 2 
		startSlicey = curY - SEARCH_SIZE / 2 
		endSlicex = curX + SEARCH_SIZE / 2 
		endSlicey = curY + SEARCH_SIZE / 2 
		if (startSlicex < 0):
			startSlicex = 0
		if (startSlicey < 0):
			startSlicey = 0
		if (endSlicex >= self.worldSize):
			endSlicex = self.worldSize - 1
		if (endSlicey >= self.worldSize):
			endSlicey = self.worldSize - 1
		goal = (int(goal[0]), int(goal[1]))
		self.bestSeen = (curX, curY)
		self.frontier.enqueue(0, (curX, curY), None, 0)
		self.startPos = (curX, curY)
		self.lastNode = (curX, curY)
		self.flagPos = translate_node_reverse((goal[0],goal[1]), -self.worldSize / 2 + startSlicex, -self.worldSize / 2 + startSlicey)
		print "self.FlagPos: ", self.flagPos
		self.nodes = self.backupNodes[startSlicex:endSlicex,startSlicey:endSlicey]
		self.penalties = self.backupPenalties[startSlicex:endSlicex,startSlicey:endSlicey]
		exists = self.iter_search()
		if not exists:
			exists = self.bestSeen #Take the closest node we searched if we can't get all the way there

		finalPath = [translate_node(exists, -self.worldSize / 2 + startSlicex, -self.worldSize / 2 + startSlicey)]
		curNode = exists
		nextNode = self.searched[exists]
		while nextNode != None:
			finalPath.insert(0, translate_node(nextNode, -self.worldSize / 2 + startSlicex, -self.worldSize / 2 + startSlicey))
			curNode = nextNode
			nextNode = self.searched[curNode]
		print "Length of final path: ", len(finalPath)
		print "Search took: ", time.time() - startTime
		return finalPath


	def step(self, steps):
		toReturn = []
		for i in range(steps):
			if not self.frontier:
				break
			done = self.iter_step()
			toReturn += self.newNodes
			if done:
				break
		return toReturn

	def iter_search(self):
		while self.frontier.has_item():
			answer = self.iter_step()
			if answer:
				return answer
		return False

	def iter_step(self):
		if (self.frontier.has_item()):
			priority, cost, curNode, parent = self.frontier.dequeue()
			if not curNode:
				return False
		else:
			return False
		if not curNode in self.searched:
			self.searched[curNode] = parent
		if (curNode[0], curNode[1]) == self.flagPos:
			return curNode
		self.check_node((curNode[0]+1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]-1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]+1), cost, curNode)
		self.check_node((curNode[0], curNode[1]-1), cost, curNode)
		self.check_node((curNode[0], curNode[1]+1), cost, curNode)
		self.check_node((curNode[0]+1, curNode[1]), cost, curNode)
		self.check_node((curNode[0]-1, curNode[1]), cost, curNode)
		return False

	def check_node(self, node, cost, parent):
		if self.in_bounds(node) and self.nodes[node] and not node in self.searched:
			newCost = cost + self.calc_cost(node, parent)
			if not self.frontier.get_data_priority(node):
				self.frontier.enqueue(newCost + self.heuristic_cost(node), node, parent, newCost)
				self.newNodes.append(node)
			elif self.frontier.get_data_priority(node) > newCost + self.heuristic_cost(node):
				self.frontier.reprioritize(newCost + self.heuristic_cost(node), node, parent, newCost)
				if node in self.searched:
					self.searched[curNode] = parent

	def in_bounds(self, node):
		return node[0] < self.nodes.shape[0] and node[0] >= 0 and node[1] < self.nodes.shape[1] and node[1] >= 0

	def heuristic_cost(self, node):
		#Our heuristic is just straight-line distance
		dist = math.hypot(node[0] - self.flagPos[0], node[1] - self.flagPos[1])
		if (dist < self.bestSeenCost):
				self.bestSeenCost = dist
				self.bestSeen = node
		return dist

	def calc_cost(self, node, parent):
		if PENALIZE:
			return self.penalties[node[0],node[1]] * math.hypot(node[0] - parent[0], node[1] - parent[1])
		else:
			return NORMAL_COST * math.hypot(node[0] - parent[0], node[1] - parent[1])


	def is_adjacent(self, node):
		if (node[0],node[1]+1) in self.nodes and (self.nodes[(node[0],node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]+1) in self.nodes and (self.nodes[(node[0]-1,node[1]+1)] == 0):
			return True
		if (node[0]-1,node[1]) in self.nodes and (self.nodes[(node[0]-1,node[1])] == 0):
			return True
		if (node[0]+1,node[1]+1) in self.nodes and (self.nodes[(node[0]+1,node[1]+1)] == 0):
			return True
		if (node[0]+1,node[1]-1) in self.nodes and (self.nodes[(node[0]+1,node[1]-1)] == 0):
			return True
		if (node[0],node[1]-1) in self.nodes and (self.nodes[(node[0],node[1]-1)] == 0):
			return True
		if (node[0]-1,node[1]-1) in self.nodes and (self.nodes[(node[0]-1,node[1]-1)] == 0):
			return True
		return False

	def clear(self):
		self.path = []
		self.frontier = PriorityQueue()
		self.frontier.enqueue(0, (self.startPos[0], self.startPos[1]), None, 0)
		self.searched = {}

def parse_grid(size, bzrc):
	toReturn = numpy.ones((size, size))
	penaltyGrid = numpy.ones((size, size), dtype=float)
	penaltyGrid.fill(NORMAL_COST)

	#Get the obstacles
	obstacles = bzrc.get_obstacles()
	otherTanks = bzrc.get_othertanks()

	#Iterate through the obstacles, drawing lines in between them
	for obstacle in obstacles:
		for i, point in enumerate(obstacle):
			#figure out which direction to go
			point = translate_node_reverse(point, -size / 2, -size / 2)
			if i+1 < len(obstacle):
				point2 = translate_node_reverse(obstacle[i+1], -size / 2, -size / 2)
			else:
				point2 = translate_node_reverse(obstacle[0], -size / 2, -size / 2)
				
			point = (int(point[0]), int(point[1]))
			point2 = (int(point2[0]), int(point2[1]))

			diffx = int(point2[0]) - int(point[0])
			diffy = int(point2[1]) - int(point[1])

			rge = {}

			if diffx < 0:
				rge = range(0, int(diffx), -1)
			elif diffx > 0:
				rge = range(0, int(diffx), 1)

			if math.fabs(diffx) > math.fabs(diffy):
				for x in rge:
					if int(point[0])+x < size and int(point[0])+x >= 0 and int(point[1]) >= 0 and int(point[1]) < size:
						toReturn[int(point[0])+x, int(point[1])] = 0
					for y in range(-PEN_DIST, PEN_DIST, 1):
						if int(point[0])+x < size and int(point[0])+x >= 0 and int(point[1])+y < size and int(point[1])+y >=0:
							penaltyGrid[int(point[0])+x, int(point[1])+y] = MAX_COST - math.fabs(y)
			else:
				if diffy < 0:
					rge = range(0, int(diffy), -1)
				else:
					rge = range(0, int(diffy), 1)
				for y in rge:
					if int(point[1])+y < size and int(point[1])+y >=0 and int(point[0]) >= 0 and int(point[0]) < size:
						toReturn[int(point[0]), int(point[1])+y] = 0
					for x in range(-PEN_DIST, PEN_DIST, 1):
						if int(point[0])+x < size and int(point[0])+x >= 0 and int(point[1])+y < size and int(point[1])+y >=0:
							penaltyGrid[int(point[0])+x, int(point[1])+y] = MAX_COST - math.fabs(x)

	#print toReturn
	#print penaltyGrid
	
	#plot_grid.init_window(size, size)
	#plot_grid.update_grid(numpy.array(toReturn))
	#plot_grid.draw_grid()

	return toReturn, penaltyGrid

def find_center(obstacle):
		return (sum([pair[0] for pair in obstacle])/len(obstacle), sum([pair[1] for pair in obstacle])/len(obstacle))

def translate_node(node, startx, starty):
	return (node[0] + startx, node[1] + starty)

def translate_node_reverse(node, startx, starty):
	return (int(node[0] - startx), int(node[1] - starty))


def main():
	# Process CLI arguments.
	parser = optparse.OptionParser(usage = "%prog [options]",
								   version = "%prog 0.1")
	parser.add_option("-o", "--host", type="string",dest="host",
			metavar="HOST",default='localhost',
			help="hostname for the server")
	parser.add_option("-p", "--port", type="int",dest="port",
			metavar="PORT",default=3907,
			help="port number for the server")

	(options, args) = parser.parse_args()

	# Connect.
	#bzrc = BZRC(options.host, int(options.port), debug=True)
	bzrc = BZRC(options.host, int(options.port))

	agent = Agent(bzrc)
	kalman = Kalman()
	kalman.start()

	prevTime = time.time()
	# Run the agent
	hit = False
	try:
		while not hit:
			hit = agent.tick()
			now = time.time()
			timeDiff = now - prevTime
			#print "Took ", timeDiff, " time to calculate."
			if (TIME_STEP > timeDiff):
				time.sleep(TIME_STEP - timeDiff)
			prevTime = time.time()

	except KeyboardInterrupt:
		print "Exiting due to keyboard interrupt."
		bzrc.close()

if __name__ == '__main__':
	main()
